# -*- coding:utf-8 -*-

import numpy as np

LIGHTSPEED         = 299792458


def satpos(w,deltatime,r,center):
    p   = center+r*np.array([np.cos(w*deltatime),np.sin(w*deltatime),0],dtype=float)
    return p

def satvelocity(w,deltatime,r):
    theta = w*deltatime
    v     =[-r*np.sin(theta)*w,r*np.cos(theta)*w,0]
    return(v)

def findrectime(emittime,wrec,wemit,rrec,remit,centerrec,centeremit):
    deltatime   = emittime
    recsatpos   = satpos(wrec,deltatime,rrec,centerrec)
    tmpemitpos  = satpos(wemit,deltatime,remit,centeremit)
    iteration=1
    distance    = np.linalg.norm(recsatpos-tmpemitpos)
    deltatranstime   = distance/LIGHTSPEED
    olddistance = distance
    olddeltatranstime    = deltatranstime
    tmpemittime = deltatime
    while True:
        tmpemittime = tmpemittime - deltatranstime
        tmpemitpos  = satpos(wemit,tmpemittime,remit,centeremit)
        distance    = np.linalg.norm(recsatpos-tmpemitpos)
        deltatranstime = -(olddistance - distance)/LIGHTSPEED
        # print   (distance/LIGHTSPEED+tmpemittime-deltatime)*LIGHTSPEED,tmpemittime
        if abs((distance/LIGHTSPEED+tmpemittime-deltatime)*LIGHTSPEED)<2e-3 or iteration>10:
            break
        else:
            olddistance = distance
            iteration  = iteration +1 
    return tmpemittime


# initweek  = 508
# initSOW   = 388804.50

if __name__ ==  '__main__':
    np.random.seed(123)

    initialTime = 508*604800+388804.50

    R  = [1e7,3e7,5e7]
    w  = 5e-3*np.array([0.99,0.20123,0.032413],dtype=float)
    centers = np.array([[0,0,0],[0,0,0],[0,0,0]],dtype=float)
    distancevar = 1e-2
    timedriftVar = 100
    filefolder = './data/sat3satObsSimTimeIssue_smallTimeDiff/'

    fhanldes = []
    for i in range(1,4):
        filename   =filefolder+'H82-Sat0'+str(i)+'-01.txt'
        fhanldes.append(open(filename,'w'))

    # tmpemitposes = np.zeros((10000,3))
    index  = 1
    for deltatime in range(1,500000,10):
        sats      =  [0,1,2]
        index1    =  np.floor(3*np.random.rand())
        emitsat   = int(index1)
        sats.pop(emitsat)

        index2    = int(np.floor(2*np.random.rand()))
        recsat      =  sats[index2]

        # timedrift  = np.sqrt(timedriftVar)*np.random.rand()  #emitSat - recSat unit: meter
        timedrift  = 1.5
        link1starttime = findrectime(deltatime,w[recsat],w[emitsat],R[recsat],R[emitsat],centers[recsat,:],centers[emitsat,:])
        link1endtime   = deltatime
        link1measurement = (link1endtime- link1starttime)*LIGHTSPEED+ np.sqrt(distancevar)*np.random.randn()-timedrift

        link2endtime  = deltatime+4
        link2starttime= findrectime(link2endtime,w[emitsat],w[recsat],R[emitsat],R[recsat],centers[emitsat,:],centers[recsat,:])
        link2measurement = (link2endtime- link2starttime)*LIGHTSPEED + np.sqrt(distancevar)*np.random.randn() + timedrift
        data = []
        data.append([0,int((initialTime+link1endtime)/604800),(initialTime+link1endtime)%604800,emitsat+1,recsat+1,link1measurement,0])

        data.append([0,int((initialTime+link2endtime)/604800),(initialTime+link2endtime)%604800,recsat+1,emitsat+1,link2measurement,0])
        str1 = ''
        for metadata in data:
            str1 = str1 + ' '.join(str(x) for x in metadata)+'\n'
        fhanldes[emitsat].write(str1)
        fhanldes[recsat].write(str1)
        # tmpemitposes[index,:] = satpos(w[recsat],deltatime,R[recsat],centers[recsat,:])
        index =  index+1

    for i in range(0,2):
        fhanldes[i].close()
        
    # initial orbit
    initialfile  = filefolder+'Init_Orbit.txt'
    f = open(initialfile,'w')

    for i in range(1,4):
        metadata = [i]
        pos =  satpos(w[i-1],0,R[i-1],centers[i-1,:])
        metadata.extend(pos)
        velocity = satvelocity(w[i-1],0,R[i-1])
        metadata.extend(velocity)
        f.write(' '.join(str(x) for x in metadata)+'\n')
    f.close()